#include "BasicType/Matrix.h"

namespace GCRE {
	namespace BasicType {
		Matrix44f::Matrix44f()
		{
			row0 = _mm_set_ps( 0.0f, 0.0f, 0.0f, 1.0f );
			row1 = _mm_set_ps( 0.0f, 0.0f, 1.0f, 0.0f );
			row2 = _mm_set_ps( 0.0f, 1.0f, 0.0f, 0.0f );
			row3 = _mm_set_ps( 1.0f, 0.0f, 0.0f, 0.0f );
		}

		Matrix44f::Matrix44f( const Matrix44f& m )
		{
			row0 = m.row0;
			row1 = m.row1;
			row2 = m.row2;
			row3 = m.row3;
		}

		Matrix44f::Matrix44f( const SFFloat* pM )
		{
			row0 = _mm_loadu_ps( pM );
			row1 = _mm_loadu_ps( pM + 4 );
			row2 = _mm_loadu_ps( pM + 8 );
			row3 = _mm_loadu_ps( pM + 12 );
		}

		void Matrix44f::SetMatrix( const SFFloat* pM )
		{
			row0 = _mm_loadu_ps( pM );
			row1 = _mm_loadu_ps( pM + 4 );
			row2 = _mm_loadu_ps( pM + 8 );
			row3 = _mm_loadu_ps( pM + 12 );
		}

		void Matrix44f::Identity()
		{
			row0 = _mm_set_ps( 0.0f, 0.0f, 0.0f, 1.0f );
			row1 = _mm_set_ps( 0.0f, 0.0f, 1.0f, 0.0f );
			row2 = _mm_set_ps( 0.0f, 1.0f, 0.0f, 0.0f );
			row3 = _mm_set_ps( 1.0f, 0.0f, 0.0f, 0.0f );
		}

		void Matrix44f::Transpose()
		{
			_MM_TRANSPOSE4_PS( row0, row1, row2, row3 );
		}

		void Matrix44f::Inverse( SFFloat epsilon /*= 1e-08 */ )
		{
			float* src = &data[0][0];
			float det_f;
			__m128 minor0, minor1, minor2, minor3, det;
			__m128 tmp = row0, r0 = row0, r1 = row1, r2 = row2, r3 = row3;

			tmp			= _mm_loadh_pi( _mm_loadl_pi( tmp, (__m64*)(src) ), (__m64*)(src+ 4) );
			r1				= _mm_loadh_pi(_mm_loadl_pi( r1, (__m64*)(src+8) ), (__m64*)(src+12) );

			r0				= _mm_shuffle_ps( tmp, r1, 0x88 );
			r1				= _mm_shuffle_ps( r1, tmp, 0xDD );

			tmp			= _mm_loadh_pi(_mm_loadl_pi( tmp, (__m64*)(src+ 2) ), (__m64*)(src+ 6) );
			r3				= _mm_loadh_pi(_mm_loadl_pi( r3, (__m64*)(src+10) ), (__m64*)(src+14) );

			r2				= _mm_shuffle_ps( tmp, r3, 0x88 );
			r3				= _mm_shuffle_ps( r3, tmp, 0xDD );

			// -----------------------------------------------
			tmp			= _mm_mul_ps( r2, r3 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0xB1 );

			minor0		= _mm_mul_ps( r1, tmp );
			minor1		= _mm_mul_ps( r0, tmp );

			tmp			= _mm_shuffle_ps( tmp, tmp, 0x4E );

			minor0		= _mm_sub_ps( _mm_mul_ps( r1, tmp ), minor0 );
			minor1		= _mm_sub_ps( _mm_mul_ps( r0, tmp ), minor1 );
			minor1		= _mm_shuffle_ps( minor1, minor1, 0x4E );
			// -----------------------------------------------
			tmp			= _mm_mul_ps( r1, r2 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0xB1 );
			minor0		= _mm_add_ps( _mm_mul_ps( r3, tmp ), minor0 );
			minor3		= _mm_mul_ps( r0, tmp );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0x4E );
			minor0		= _mm_sub_ps( minor0, _mm_mul_ps( r3, tmp ) );
			minor3		= _mm_sub_ps( _mm_mul_ps( r0, tmp ), minor3 );
			minor3		= _mm_shuffle_ps( minor3, minor3, 0x4E );
			// -----------------------------------------------	
			tmp			= _mm_mul_ps( _mm_shuffle_ps( r1, r1, 0x4E), r3 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0xB1 );
			r2				= _mm_shuffle_ps( r2, r2, 0x4E );

			minor0		= _mm_add_ps( _mm_mul_ps( r2, tmp ), minor0 );
			minor2		= _mm_mul_ps( r0, tmp );

			tmp			= _mm_shuffle_ps( tmp, tmp, 0x4E );

			minor0		= _mm_sub_ps( minor0, _mm_mul_ps( r2, tmp ) );
			minor2		= _mm_sub_ps( _mm_mul_ps( r0, tmp ), minor2 );
			minor2		= _mm_shuffle_ps( minor2, minor2, 0x4E );
			// -----------------------------------------------
			tmp			= _mm_mul_ps( r0, r1 );	
			tmp			= _mm_shuffle_ps( tmp, tmp, 0xB1 );
			minor2		= _mm_add_ps( _mm_mul_ps( r3, tmp ), minor2 );
			minor3		= _mm_sub_ps( _mm_mul_ps( r2, tmp ), minor3 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0x4E );
			minor2		= _mm_sub_ps( _mm_mul_ps( r3, tmp ), minor2 );
			minor3		= _mm_sub_ps( minor3, _mm_mul_ps( r2, tmp ) );
			// -----------------------------------------------
			tmp			= _mm_mul_ps( r0, r3 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0xB1 );

			minor1		= _mm_sub_ps( minor1, _mm_mul_ps( r2, tmp ) );
			minor2		= _mm_add_ps( _mm_mul_ps( r1, tmp ), minor2 );

			tmp			= _mm_shuffle_ps( tmp, tmp, 0x4E );

			minor1		= _mm_add_ps( _mm_mul_ps( r2, tmp ), minor1 );
			minor2		= _mm_sub_ps( minor2, _mm_mul_ps( r1, tmp ) );
			// -----------------------------------------------
			tmp			= _mm_mul_ps( r0, r2 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0xB1 );

			minor1		= _mm_add_ps( _mm_mul_ps( r3, tmp ), minor1 );
			minor3		= _mm_sub_ps( minor3, _mm_mul_ps( r1, tmp ) );

			tmp			= _mm_shuffle_ps( tmp, tmp, 0x4E );

			minor1		= _mm_sub_ps( minor1, _mm_mul_ps( r3, tmp ) );
			minor3		= _mm_add_ps( _mm_mul_ps( r1, tmp ), minor3 );
			// -----------------------------------------------
			det			= _mm_mul_ps( r0, minor0 );
			det			= _mm_add_ps( _mm_shuffle_ps( det, det, 0x4E ), det );
			det			= _mm_add_ss( _mm_shuffle_ps( det, det, 0xB1 ), det );
			tmp			= _mm_rcp_ss( det );

			_mm_store_ss( &det_f, det );
			assert( det_f > epsilon );

			det			= _mm_sub_ss( _mm_add_ss( tmp, tmp ), _mm_mul_ss( det, _mm_mul_ss( tmp, tmp ) ) );
			det			= _mm_shuffle_ps( det, det, 0x00 );

			minor0		= _mm_mul_ps( det, minor0 );
			_mm_storel_pi( (__m64*)(src), minor0 );
			_mm_storeh_pi( (__m64*)(src+2), minor0 );

			minor1 = _mm_mul_ps( det, minor1 );
			_mm_storel_pi( (__m64*)(src+4), minor1 );
			_mm_storeh_pi( (__m64*)(src+6), minor1 );

			minor2 = _mm_mul_ps( det, minor2 );
			_mm_storel_pi( (__m64*)(src+ 8), minor2 );
			_mm_storeh_pi( (__m64*)(src+10), minor2 );

			minor3 = _mm_mul_ps( det, minor3 );
			_mm_storel_pi( (__m64*)(src+12), minor3 );
			_mm_storeh_pi( (__m64*)(src+14), minor3 );
		}

		SFFloat Matrix44f::Determinant() const
		{
			const float* src = &data[0][0];
			float det_f;
			__m128 minor0, det;
			__m128 tmp = row0, r0 = row0, r1 = row1, r2 = row2, r3 = row3;

			tmp			= _mm_loadh_pi( _mm_loadl_pi( tmp, (__m64*)(src) ), (__m64*)(src+ 4) );
			r1				= _mm_loadh_pi(_mm_loadl_pi( r1, (__m64*)(src+8) ), (__m64*)(src+12) );

			r0				= _mm_shuffle_ps( tmp, r1, 0x88 );
			r1				= _mm_shuffle_ps( r1, tmp, 0xDD );

			tmp			= _mm_loadh_pi(_mm_loadl_pi( tmp, (__m64*)(src+ 2) ), (__m64*)(src+ 6) );
			r3				= _mm_loadh_pi(_mm_loadl_pi( r3, (__m64*)(src+10) ), (__m64*)(src+14) );

			r2				= _mm_shuffle_ps( tmp, r3, 0x88 );
			r3				= _mm_shuffle_ps( r3, tmp, 0xDD );

			// -----------------------------------------------
			tmp			= _mm_mul_ps( r2, r3 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0xB1 );

			minor0		= _mm_mul_ps( r1, tmp );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0x4E );
			minor0		= _mm_sub_ps( _mm_mul_ps( r1, tmp ), minor0 );
			// -----------------------------------------------
			tmp			= _mm_mul_ps( r1, r2 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0xB1 );
			minor0		= _mm_add_ps( _mm_mul_ps( r3, tmp ), minor0 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0x4E );
			minor0		= _mm_sub_ps( minor0, _mm_mul_ps( r3, tmp ) );
			// -----------------------------------------------	
			tmp			= _mm_mul_ps( _mm_shuffle_ps( r1, r1, 0x4E), r3 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0xB1 );
			r2				= _mm_shuffle_ps( r2, r2, 0x4E );

			minor0		= _mm_add_ps( _mm_mul_ps( r2, tmp ), minor0 );
			tmp			= _mm_shuffle_ps( tmp, tmp, 0x4E );
			minor0		= _mm_sub_ps( minor0, _mm_mul_ps( r2, tmp ) );

			det			= _mm_mul_ps( r0, minor0 );
			det			= _mm_add_ps( _mm_shuffle_ps( det, det, 0x4E ), det );
			det			= _mm_add_ss( _mm_shuffle_ps( det, det, 0xB1 ), det );

			_mm_store_ss( &det_f, det );

			return det_f;
		}

		void Matrix44f::SetColumn( SFInt col, const Vec4f& v )
		{
			assert( col >= 0 && col < 4 );
			data[0][col] = v[0];
			data[1][col] = v[1];
			data[2][col] = v[2];
			data[3][col] = v[3];
		}

		void Matrix44f::SetColumn( SFInt col, const SFFloat* c )
		{
			assert( col >= 0 && col < 4 );
			data[0][col] = c[0];
			data[1][col] = c[1];
			data[2][col] = c[2];
			data[3][col] = c[3];
		}

		void Matrix44f::SetRow( SFInt row, const Vec4f& v )
		{
			assert( row >= 0 && row < 4 );
			__m128 *rows = &row0;
			rows[row] = _mm_set_ps( v[3], v[2], v[1], v[0] );
		}

		void Matrix44f::SetRow( SFInt row, const SFFloat* r )
		{
			assert( row >= 0 && row < 4 );
			__m128 *rows = &row0;
			rows[row] = _mm_set_ps( r[3], r[2], r[1], r[0] );
		}

		Vec4f Matrix44f::GetColumn( SFInt col ) const
		{
			assert( col >= 0 && col < 4 );
			return Vec4f( data[0][col], data[1][col], data[2][col], data[3][col] );
		}

		Vec4f Matrix44f::GetRow( SFInt row ) const
		{
			assert( row >= 0 && row < 4 );
			return Vec4f( data[row][0], data[row][1], data[row][2], data[row][3] );
		}

		void Matrix44f::Transform( Vec4f& v ) const
		{
			v = v * (*this);
		}

		void Matrix44f::Transform( Vec3f& v, SFFloat w ) const
		{
			Vec4f v4( v, w );
			v4 = v4 * (*this);
			v[0] = v4[0];
			v[1] = v4[1];
			v[2] = v4[2];
		}

		Matrix44f& Matrix44f::operator=( const Matrix44f& m )
		{
			row0 = m.row0;
			row1 = m.row1;
			row2 = m.row2;
			row3 = m.row3;

			return *this;
		}

		Matrix44f Matrix44f::operator+( const Matrix44f& m ) const
		{
			Matrix44f result;

			result.row0 = _mm_add_ps( row0, m.row0 );
			result.row1 = _mm_add_ps( row1, m.row1 );
			result.row2 = _mm_add_ps( row2, m.row2 );
			result.row3 = _mm_add_ps( row3, m.row3 );

			return result;
		}

		Matrix44f Matrix44f::operator-( const Matrix44f& m ) const
		{
			Matrix44f result;

			result.row0 = _mm_sub_ps( row0, m.row0 );
			result.row1 = _mm_sub_ps( row1, m.row1 );
			result.row2 = _mm_sub_ps( row2, m.row2 );
			result.row3 = _mm_sub_ps( row3, m.row3 );

			return result;
		}

		Matrix44f& Matrix44f::operator+=( const Matrix44f& m )
		{
			row0 = _mm_add_ps( row0, m.row0 );
			row1 = _mm_add_ps( row1, m.row1 );
			row2 = _mm_add_ps( row2, m.row2 );
			row3 = _mm_add_ps( row3, m.row3 );

			return *this;
		}

		Matrix44f& Matrix44f::operator-=( const Matrix44f& m )
		{
			row0 = _mm_sub_ps( row0, m.row0 );
			row1 = _mm_sub_ps( row1, m.row1 );
			row2 = _mm_sub_ps( row2, m.row2 );
			row3 = _mm_sub_ps( row3, m.row3 );

			return *this;
		}

		Matrix44f Matrix44f::operator*( SFFloat f ) const
		{
			Matrix44f result;
			__m128 ffff = _mm_set_ps1( f );

			result.row0 = _mm_mul_ps( row0, ffff );
			result.row1 = _mm_mul_ps( row1, ffff );
			result.row2 = _mm_mul_ps( row2, ffff );
			result.row3 = _mm_mul_ps( row3, ffff );

			return result;
		}

		Matrix44f Matrix44f::operator*( const Matrix44f& m ) const
		{
			Matrix44f result;
			Vec4f v;

			v.vec = row0;
			v = v * m;
			result.row0 = v.vec;

			v.vec = row1;
			v = v * m;
			result.row1 = v.vec;

			v.vec = row2;
			v = v * m;
			result.row2 = v.vec;

			v.vec = row3;
			v = v * m;
			result.row3 = v.vec;

			return result;
		}

		Vec3f operator*( const Vec3f& v, const Matrix44f& m )
		{
			Vec4f V( v, 1.0f );
			V = V * m;
			if ( V[3] != 0.0f && V[3] != 1.0f )
				V /= V[3];
			return Vec3f( V.x(), V.y(), V.z() );
		}

		Vec4f operator*( const Vec4f& v, const Matrix44f& m )
		{
			Vec4f result;
			__m128 xxxx = _mm_shuffle_ps( v.vec, v.vec, _MM_SHUFFLE(0,0,0,0) );
			__m128 yyyy = _mm_shuffle_ps( v.vec, v.vec, _MM_SHUFFLE(1,1,1,1) );
			__m128 zzzz = _mm_shuffle_ps( v.vec, v.vec, _MM_SHUFFLE(2,2,2,2) );
			__m128 wwww = _mm_shuffle_ps( v.vec, v.vec, _MM_SHUFFLE(3,3,3,3) );

			__m128 r0 = _mm_mul_ps( m.row0, xxxx );
			__m128 r1 = _mm_mul_ps( m.row1, yyyy );
			__m128 r2 = _mm_mul_ps( m.row2, zzzz );
			__m128 r3 = _mm_mul_ps( m.row3, wwww );

			result.vec = _mm_add_ps( r0, r1 );
			result.vec = _mm_add_ps( result.vec, r2 );
			result.vec = _mm_add_ps( result.vec, r3 );

			return result;
		}

		Matrix44f Matrix44f::operator/( SFFloat f ) const
		{
			Matrix44f result;
			__m128 ffff = _mm_set_ps1( f );

			result.row0 = _mm_div_ps( row0, ffff );
			result.row1 = _mm_div_ps( row1, ffff );
			result.row2 = _mm_div_ps( row2, ffff );
			result.row3 = _mm_div_ps( row3, ffff );

			return result;
		}

		Matrix44f& Matrix44f::operator*=( SFFloat f )
		{
			__m128 ffff = _mm_set_ps1( f );

			row0 = _mm_mul_ps( row0, ffff );
			row1 = _mm_mul_ps( row1, ffff );
			row2 = _mm_mul_ps( row2, ffff );
			row3 = _mm_mul_ps( row3, ffff );

			return *this;
		}

		Matrix44f& Matrix44f::operator/=( SFFloat f )
		{
			__m128 ffff = _mm_set_ps1( f );

			row0 = _mm_div_ps( row0, ffff );
			row1 = _mm_div_ps( row1, ffff );
			row2 = _mm_div_ps( row2, ffff );
			row3 = _mm_div_ps( row3, ffff );

			return *this;
		}

		void* Matrix44f::operator new( size_t size )
		{
			return _mm_malloc( size, 16 );
		}

		void* Matrix44f::operator new[]( size_t size )
		{
			return _mm_malloc( size, 16 );
		}

		void Matrix44f::Print()
		{
			for ( int i = 0; i < 4; i++ )
			{
				for ( int j = 0; j < 4; j++ )
				{
					printf( "%f ", data[i][j] );
				}
				printf( "\n" );
			}
		}

		void Matrix44f::operator delete[]( void *mem )
		{
			_mm_free( mem );
		}

		void Matrix44f::operator delete( void *mem )
		{
			_mm_free( mem );
		}

	}
}